//!Includes

#include "IBVScontrol.h"

//!Global Functions


namespace algorithms
{

/**
 * class IBVScontrol
 *
 *
 * Autor:David Bevilaqua
 *
 *
 * DESCRIPTION:
 *
 */

//!Constructors/Destructors

/**
 * Empty Constructor
 */
IBVScontrol::IBVScontrol ( )
{
	initAttributes();
}

/**
 * Empty Destructor
 */
IBVScontrol::~IBVScontrol ( )
{

}

//!Public Accessor Methods
void IBVScontrol::SetTargetVecotor(std::vector <algorithms::Target> TargetVector)
{
	m_TargetVector=TargetVector;
}

int IBVScontrol::GetVelWheelLeft()
{
	if(m_iVelWheelLeft>300)
	{
		m_iVelWheelLeft=300;
	}
	if(m_iVelWheelLeft<-300)
	{
		m_iVelWheelLeft=-300;
	}
	return m_iVelWheelLeft;
}

int IBVScontrol::GetVelWheelRight()
{
	if(m_iVelWheelRight>300)
	{
		m_iVelWheelRight=300;
	}
	if(m_iVelWheelRight<-300)
	{
		m_iVelWheelRight=-300;
	}
	return m_iVelWheelRight;
}


int IBVScontrol::GetVelLinear()
{
	return m_iVelLinear;
}

int IBVScontrol::GetVelRot()
{
	return m_iVelRot;
}

bool IBVScontrol::StopRobot()
{

	return m_bStopRobot;
}

bool IBVScontrol::GetAchieveTarget()
{
return m_bAchieveTarget;
}

void IBVScontrol::SetAchieveTarget(bool val)
{
m_bAchieveTarget=val;
}
//!Public Methods

void IBVScontrol::LostTarget(bool lost)
{
	m_bLost=lost;
}

int IBVScontrol::SetErroY ()
{
	//if(m_ErroX<m_ErroTheta&&m_ErroX>50){
	//m_ErroTheta=m_ErroX;
	//}
	if(m_ErroY<5){
	m_ErroTheta=0;
	}

	if(	m_TargetVector.back().GetDiffDiagonal()>200)
	{
	return 200;
	}
	else
	{
    return m_TargetVector.back().GetDiffDiagonal();
	}
}

int IBVScontrol::SetErroX ()
{
	if(	m_TargetVector.back().GetDiffCenterX()>50)
	{
		return 50;
	}
	else if(m_TargetVector.back().GetDiffCenterX()<-50)
	{
		return -50;
	}
	else
	{
	return m_TargetVector.back().GetDiffCenterX();
	}
}

int IBVScontrol::SetErroTh ()
{
	if(m_TargetVector.back().GetDiffOrientation()>40&&m_TargetVector.back().GetDiffEstOrientation()>0)
	{
		return -40;
	}
	else if(m_TargetVector.back().GetDiffOrientation()>40&&m_TargetVector.back().GetDiffEstOrientation()<0)
	{
		return 40;
	}
	else if(m_TargetVector.back().GetDiffEstOrientation()>0)
	{
		return -m_TargetVector.back().GetDiffOrientation();
	}
	else if(m_TargetVector.back().GetDiffEstOrientation()<0)
	{
		return m_TargetVector.back().GetDiffOrientation();
	}
	else
	{
	return 0;
	}
}

void IBVScontrol::Execute()
{
	m_ErroTheta=SetErroTh();
	m_ErroX=SetErroX ();
	m_ErroY=SetErroY ();
	//printf("x:%d,y:%d,th:%d\n",m_TargetVector.back().GetPoseRobot().GetPose().x,m_TargetVector.back().GetPoseRobot().GetPose().y,m_TargetVector.back().GetPoseRobot().GetOrientation());
	if(m_bLost)
	{
		SetVelLinear(m_dbKp*(double)m_ErroY);
		SetVelAngular(m_dbKa*(double)m_ErroX-m_dbKa*(double)m_ErroTheta);

	}
	else
	{
		SetVelLinear(m_dbKp*(double)m_ErroY);
		SetVelAngular(m_dbKa*(double)m_ErroX-m_dbKb*(double)m_ErroTheta);


	}
	//printf("m_ErroX: %d m_ErroY: %d \n",m_ErroX,m_ErroY);
	//printf("m_ErroTheta: %d \n",m_ErroTheta);
	//printf("m_iVelWheelLeft:%d m_iVelWheelRight: %d \n",m_iVelWheelLeft,m_iVelWheelRight);
	if(std::abs(m_ErroX)>=5||std::abs(m_ErroY)>=5)
	{
		m_bStopRobot=false;
	}
	else
	{
		m_bAchieveTarget=true;
		m_bStopRobot=true;
	}
	m_TargetVector.clear();
}

//!Protected Methods



//!Private Methods




/**
 * Starts all the attributes of
 * the IBVScontrol
 */
void IBVScontrol::SetVelLinear(int val)
{

	if(val>200)
	{
		val=200;
	}
	else if(val<-200)
	{
		val=-200;
	}
	m_iVelLinear=val;

	m_iVelWheelLeft=val;
	m_iVelWheelRight=val;
}

void IBVScontrol::SetVelAngular(int val)
{

	if(val>30)
	{
		val=30;
	}
	else if(val<-30)
	{
		val=-30;
	}
	m_iVelRot=val;

	m_iVelWheelLeft=m_iVelWheelLeft-val;
	m_iVelWheelRight=m_iVelWheelRight+val;

}

void IBVScontrol::initAttributes()
{

m_bAchieveTarget=false;
	m_bStopRobot=false;
	m_iVelWheelLeft=0;
	m_iVelWheelRight=0;
	m_dbKp=3;
	m_dbKa=-0.2;
	m_dbKb=-0.0;
}


}
